﻿using System;
using BulletSharp;

namespace Robot
{
    class Leg
    {
        Vector3 vUp = new Vector3(0, 1, 0);
        const float PI2 = (float)(2f * Math.PI);
        const float PI_2 = (float)(0.5f * Math.PI);
        const float PI_4 = (float)(0.25f * Math.PI);
        const float PI_8 = (float)(0.125f * Math.PI);

        RigidBody robotBody;

        const float bodySize = 0.25f;
        const float legLength = 0.45f;
        const float foreLegLength = 0.75f;
        Vector3 up = new Vector3(0, 1, 0);

        RigidBody body1;
        RigidBody body2;

        CollisionShape shape1;
        CollisionShape shape2;

        HingeConstraint joint1;
        HingeConstraint joint2;
        HingeConstraint joint3;

        public Leg(DynamicsWorld world, RigidBody robotBody, float depth, float fHeight, Matrix offset, float fAngle)
        {
            const float fBodySize = 0.25f;
            const float fLegLength = 0.45f;
            const float fForeLegLength = 0.75f;


            shape1 = new CapsuleShape(0.10f, fLegLength);
            shape2 = new CapsuleShape(0.08f, fForeLegLength);
            Vector3 vRoot = new Vector3(0, fHeight, 0);

            float fSin = (float)Math.Sin(fAngle);
            float fCos = (float)Math.Cos(fAngle);

            Vector3 vBoneOrigin = new Vector3(fCos * (fBodySize + 0.5f * fLegLength), fHeight, fSin * (fBodySize + 0.5f * fLegLength));

            // thigh
            Vector3 vToBone = (vBoneOrigin - vRoot);
            vToBone.Normalize();
            Vector3 vAxis = Vector3.Cross(vToBone, vUp);
            Matrix transform;
            transform = Matrix.RotationQuaternion(Quaternion.RotationAxis(vAxis, PI_2)) * Matrix.Translation(vBoneOrigin);
            body1 = RobotModel.LocalCreateRigidBody(0.5f, world, transform * offset, shape1);

            // shin
            transform = Matrix.Translation(fCos * (fBodySize + fLegLength), fHeight - 0.5f * fForeLegLength, fSin * (fBodySize + fLegLength));
            body2 = RobotModel.LocalCreateRigidBody(0.5f, world, transform * offset, shape2);


            body1.SetDamping(0.05f, 0.85f);
            body1.DeactivationTime = 0.8f;
            //boddy1.SetSleepingThresholds(1.6f, 2.5f);
            body1.SetSleepingThresholds(0.5f, 0.5f);

            body2.SetDamping(0.05f, 0.85f);
            body2.DeactivationTime = 0.8f;
            //boddy1.SetSleepingThresholds(1.6f, 2.5f);
            body2.SetSleepingThresholds(0.5f, 0.5f);

            Matrix localA, localB, localC;

            // hip joints
            localA = Matrix.RotationYawPitchRoll(-fAngle, 0, 0) * Matrix.Translation(fCos * fBodySize, 0, fSin * fBodySize); // OK
            localB = localA * robotBody.WorldTransform * Matrix.Invert(body1.WorldTransform);
            joint1 = new HingeConstraint(robotBody, body1, localA, localB);
            joint1.SetLimit(-0.75f * PI_2, PI_8);
            //hingeC.SetLimit(-0.1f, 0.1f);
            world.AddConstraint(joint1, true);
            /*
            //hip horisontal joint
            localA = Matrix.RotationYawPitchRoll(-fAngle, PI_2, 0) * Matrix.Translation(fCos * fBodySize, 0, fSin * fBodySize); // OK
            localB = localA * robotBody.WorldTransform * Matrix.Invert(body1.WorldTransform);
            joint3 = new HingeConstraint(robotBody, body1, localA, localB);
            joint3.SetLimit(-PI_4, PI_4);
            world.AddConstraint(joint3, true);
            */
            // knee joints
            localA = Matrix.RotationYawPitchRoll(-fAngle + PI_2, 0, -PI_2) * Matrix.Translation(fCos * (fBodySize + fLegLength), 0, fSin * (fBodySize + fLegLength));
            localB = localA * robotBody.WorldTransform * Matrix.Invert(body1.WorldTransform);
            localC = localA * robotBody.WorldTransform * Matrix.Invert(body2.WorldTransform);
            joint2 = new HingeConstraint(body1, body2, localB, localC);
            //hingeC.SetLimit(-0.01f, 0.01f);
            //joint2.SetLimit(-PI_8, 0.2f);
            joint2.SetLimit(-PI_4, PI_4);
            world.AddConstraint(joint2, true);

            //period = (float)Robot.R.NextDouble();
        }

        float time = 0;
        public float period;
        public float period2;

        public void Update(float timeStep)
        {
            float ms = timeStep * 1000000.0f;
            time += ms;
            
            UpdateLimb(ms, joint1, period);
            UpdateLimb(ms, joint2, period2);
            

        }

        public void UpdateLimb(float ms, HingeConstraint joint, float per)
        {

            float lowerLimit = joint.LowerLimit;
            float upperLimit = joint.UpperLimit;
            float currentAngle = joint.HingeAngle;

            const float muscleStrength = 100f;
            float fCyclePeriod = 500 +5000 * per;
            if (fCyclePeriod < 100) fCyclePeriod = 100f;
            float fTargetPercent = ((int)(time / 1000.0f) % (int)fCyclePeriod) / fCyclePeriod;

            float fTargetAngle = (float)(0.5 * (1 + Math.Sin(PI2 * fTargetPercent)));
            float fTargetLimitAngle = lowerLimit + fTargetAngle * (upperLimit - lowerLimit);
            float fAngleError = fTargetLimitAngle - currentAngle;
            float fDesiredAngularVel = 1000000.0f * fAngleError / ms;
            joint.EnableAngularMotor(true, fDesiredAngularVel, muscleStrength);
        }

        public void Remove(DynamicsWorld world)
        {
            world.RemoveRigidBody(body1);
            world.RemoveRigidBody(body2);
            world.RemoveConstraint(joint1);
            world.RemoveConstraint(joint2);
            //world.RemoveConstraint(joint3);
        }

    }
}
